// net_vision.h
//
// Network interface for vision socket.  See socket.h for info
// on connecting a client. 
//
// Given a connected client, here's some example code...
//
// net_vframe frame;
//
// while(client->ready_for_read()) {
//   client->recv(&frame, sizeof(frame));
//   ...
// }
// ...
// vref ref = { NET_VISION_REF, REF_STOP };
// client->send(&ref, sizeof(ref));
// ...
//
// Created by:  Michael Bowling (mhb@cs.cmu.edu)
//              Adapted from CMDragons 2001 (jbruce@cs.cmu.edu)
//
/* LICENSE: */

#ifndef __net_vision_h__
#define __net_vision_h__

#include <stdint.h>

#include "../include/constants.h"
#include "../include/rtypes.h"
#include "../include/utils/macros.h"
#include "../include/utils/vector.h"

// network port, protocol and acknowledgement period
#define NET_VISION_PROTOCOL   Socket::UDP
#define NET_VISION_ACK_PERIOD 30
#define NET_VISION_PORT       32881

static const unsigned short VisionFrameMsgId  = 0x0001;
static const unsigned short SubscribeMsgId    = 0x0101;
static const unsigned short RobotCommandMsgId = 0x0102;
static const unsigned short VisionConfigMsgId = 0x0103;
static const unsigned short RadioControlMsgId = 0x0104;
static const unsigned short SimRefMsgId       = 0x0201;
static const unsigned short SimMoveRobotMsgId = 0x0202;
static const unsigned short SimMoveBallMsgId  = 0x0203;

// Base type for TLV (type,length,value) messages
struct NetMsg{
  uint16_t msg_type;
  uint16_t msg_len;
}; // 2*2 = 4

struct NetTimedMsg : public NetMsg{
  double timestamp;
}; // 4 + 8 = 12

//==== Output Messages ===============================================//

struct VisionRaw{
  double timestamp;
  vector2f pos;
  float angle;
  float conf;
}; // 8 + 4*2 + 4*2 = 24

struct VisionBall{
  struct { float x; float y; float vx; float vy; } state;
  float variances[16];

  VisionRaw vision;

  vector2f occluding_offset;
  char occluded;
  char occluding_team, occluding_robot;
  char reserved; // default to 0
}; // 20*4 + 24 + 8 + 3+1 = 116

struct VisionRobot{
  struct { 
    float x; float y; float theta; 
    float vx; float vy; float vtheta;
    float stuck; } state;
  VisionRaw vision;
}; // 7*4 + (4 + 24) = 52

struct VisionConfig{
  int8_t id[NUM_TEAMS][MAX_TEAM_ROBOTS];
  uint8_t cover_type[NUM_TEAMS]; // type of visual tracking covers

  static const uchar CoverNone      = 0;
  static const uchar CoverCMDragons = 1;
/*
  static const uchar CoverButterfly = 1;
  static const uchar CoverSimple    = 2;
  static const uchar CoverLinear    = 3;
  static const uchar CoverTriangle  = 4;
*/

  bool isRobotActive(int team,int index) const
    {return(id[team][index] >= 0);}
  int getRobotID(int team,int index) const
    {return(id[team][index]);}
  int getRobotType(int team,int index) const
    {return((cover_type[team]==CoverCMDragons)?
	    ROBOT_TYPE(id[team][index]) : ROBOT_TYPE_NONE);}
}; // 5*2 + 2 = 12

struct VisionFrameMsg : public NetTimedMsg {
  VisionBall ball;
  VisionRobot robots[NUM_TEAMS][MAX_TEAM_ROBOTS];
 
  VisionConfig config;

  uint8_t ref_state;   // last valid referee character sent
  uint8_t kicked;      // (bool) has ball been moved since last ref command?
  uint8_t reserved[2]; // reserved, should default to 0
}; // 12 + 116 + 52*2*5 + 12 + 2 = 664

const int net_vision_out_maxsize = sizeof(VisionFrameMsg);

// depricated names (support for a while at least)
typedef VisionFrameMsg net_vframe;
typedef VisionRaw      vraw;
typedef VisionBall     vball;
typedef VisionRobot    vrobot;


//==== Input Messages ================================================//

struct RobotCommand{
  uint8_t id;       // physical robot ID
  uint8_t priority; // higher # is higher priority
  int8_t dribble;   // dribbler speed [-128..127], 0 is off

  int8_t attr; // attrbutes
  static const int AttrKick       = 1<<1; // fire the kicker
  static const int AttrSleep      = 1<<2; // stop the servo (conserves power)
  static const int AttrAngleServo = 1<<3; // ang position servo (not ang vel)

  float vx,vy,va; // velocity in mm/sec, rad/sec
}; // 4+3*4 = 16

struct RobotCommandMsg : public NetTimedMsg {
  uint8_t team;        // team these robots are on
  uint8_t num_cmds;    // number of valid commands
  uint8_t reserved[2]; // reserved, should default to zero
  RobotCommand cmd[MAX_TEAM_ROBOTS];

  int size(int num_robots)
    {return(sizeof(NetTimedMsg)+4+sizeof(RobotCommand)*num_robots);}
}; // 12 + 4 + 16*5 = 96

struct VisionConfigMsg : public NetMsg{
  VisionConfig config;
}; // 4 + 12 = 16

#define VCR_PAUSE    0
#define VCR_PLAY     1
#define VCR_STEP     2
#define VCR_SLOW     3

#define VCR_FFWD     4
#define VCR_RWND     5

#define VCR_PLAYBACK 6
#define VCR_STEPBACK 7

struct RadioControlMsg : public NetMsg {
  uint8_t control;  // Control command (see above defines)
  uint8_t reserved; // reserved, should default to zero
}; // 4 + 1 + 1 = 6

struct SimRefMsg : public NetMsg {
  unsigned char ref_state;
  unsigned char reserved[3]; // reserved, should default to zero
}; // 4 + 4 = 8

struct SimMoveRobotMsg : public NetMsg {
  vector2f_struct loc;
  float angle;
  uchar team, id;
  unsigned char reserved[2]; // reserved, should default to zero
}; // 4 + 4*2 + 4 + 2 + 2 = 20

struct SimMoveBallMsg : public NetMsg {
  vector2f_struct loc;
  vector2f_struct vel;
}; // 4 + 4*2 + 4*2 = 20

union VisionMsg{
  NetMsg          msg;
  RobotCommandMsg robot_command;
  VisionConfigMsg vision_config;
  RadioControlMsg radio_control;
  SimRefMsg       sim_ref;
  SimMoveRobotMsg sim_move_robot;
  SimMoveBallMsg  sim_move_ball;
}; // (RobotCommandMsg) = 96

static const int net_vision_in_maxsize = sizeof(VisionMsg);

//==== Vision-only protocol ==========================================//

struct VisionPos2{
  double timestamp;
  vector2f loc;
  float angle;
  float conf;
}; // 8 + 4*4 = 24

struct VisionRobot2 : public VisionPos2 {
  uchar id;   // Robot ID#, or 0 if unknown

  uchar attr; // attributes
  static const uchar IsEnabled = 1<<0;
  static const uchar IsOmni    = 1<<1;
  static const uchar HasAngle  = 1<<2;

  uchar reserved[2];
}; // 24 + 4 = 28

struct VisionFrameMsg2 : public NetTimedMsg {
  VisionRobot2 robot[NUM_TEAMS][MAX_TEAM_ROBOTS];
  VisionPos2 ball;

  unsigned frame_seq_num; // wrapping frame counter
  uchar ref_state; // state of referee
  unsigned char reserved[3]; // default should be 0  
}; // 8 + 2*5*32 + 24 + 8 + 4 + 4 = 368

/*
struct SubscribeMsg : public NetMsg {
  unsigned magic;

  unsigned subs;
  static const uchar SubVision       = 1<<0;
  static const uchar SubCmdsBlue     = 1<<1;
  static const uchar SubCmdsYellow   = 1<<2;
  static const uchar SubPerformance  = 1<<3;

  unsigned attr;
  static const uchar AttrHighPriority = 1<<0;
}; // 4 + 3*4 = 16

struct RobotCommand{
  uchar id; // physical robot ID

  uchar attr; // attributes
  static const uchar Kick         = 1<<0;
  static const uchar Dribble      = 1<<1;
  static const uchar Sleep        = 1<<2;
  static const uchar LowPriority  = 1<<3;
  static const uchar HighPriority = 1<<4;

  // ms after timestamp that command was first sent to serial/radio
  // (used in vision frame command report, sender should set to 0)
  unsigned short sent_msec;

  float dx,dy,da; // velocity in mm/sec and rad/sec
}; // 4+3*4 = 16
*/

#endif
